#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     sonar2,         sensorSONAR)
#pragma config(Sensor, S3,     HTSPB,          sensorI2CCustom9V)
#pragma config(Motor,  motorA,          hopperNXT,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     left,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     right,         tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     hopper,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     fingers,       tmotorTetrix, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define sonar4 0
#define servo1 0
#define servo2 0

// based on PSP-Nx-motor-control.c
// http://www.mindsensors.com/index.php?module=documents&JAS_DocumentManager_op=viewDocument&JAS_Document_id=13

const ubyte Addr = 0x02;
const tSensors SensorPort = S4;        // Connect PSPNX sensorto this port!!
#include "PSP-Nx-lib.h"
#include "geary-nomux-stuff.c"

int nLeftButton = 0;
int nRightButton = 0;
int nEnterButton = 0;
int nExitButton = 0;

const int DeadZone = 15;

task
main ()
{
  int powerLeft, powerRight;
  int d_left_X, d_left_Y;
  int d_right_X, d_right_Y;
  int hopper_up, hopper_down, hopper_score;
  int fingers_up, fingers_down, fingers_up_encoder, fingers_down_encoder;
  int trigger_pressed;
  int ok_to_drive = false;
  int joystick_click;

	psp currState;

  nNxtExitClicks = 3;                // Triple clicking EXIT button will terminate program
  nI2CBytesReady[SensorPort] = 0;
  SensorType[SensorPort] = sensorI2CMuxController;
  wait10Msec (100);

  while (true) {
  PSP_ReadButtonState(SensorPort, Addr, currState);

      // joysticks
      d_left_X = (int)currState.l_j_x;
      d_left_Y = (int)currState.l_j_y;
      d_right_X = (int)currState.r_j_x;
      d_right_Y = (int)currState.r_j_y;

      // left button pad
      hopper_down = !(bool)currState.d;
      hopper_up = !(bool)currState.c || !(bool)currState.a;
      hopper_score = !(bool)currState.b;

      // right button pad
      fingers_up_encoder = !(bool)currState.triang;
      fingers_down_encoder = !(bool)currState.cross;
      fingers_up = !(bool)currState.square;
      fingers_down = !(bool)currState.circle;

      // any of the trigger buttons
      trigger_pressed = (!(bool)currState.l1) || (!(bool)currState.l2)
                     || (!(bool)currState.r1) || (!(bool)currState.r2);

      joystick_click = !(bool)currState.l_j_b || !(bool)currState.r_j_b;

	  	nxtDisplayTextLine(1,"Left: %d", d_left_Y);
  		nxtDisplayTextLine(2,"Right: %d", d_right_Y);
  		nxtDisplayTextLine(3,"Hopper: %d", hopper_score);
  		nxtDisplayTextLine(4,"Fingers: %d", fingers_down);
  		nxtDisplayTextLine(5,"Triggers: %d", trigger_pressed);
  		nxtDisplayTextLine(5,"Joy Click: %d", joystick_click);

}
  wait10Msec (100);

  StopAllTasks ();
}
